import numpy as np

const_head_vuer_mat = np.array([[1, 0, 0, 0],     #  left-right
                                [0, 1, 0, 1.5],   #  height
                                [0, 0, 1, -0.2],  #  forward
                                [0, 0, 0, 1]])
# For left wrist initial position
const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.23],  #  left-right
                                      [0, 1, 0, 1.23],   #  height
                                      [0, 0, 1, -0.4],  #  forward
                                      [0, 0, 0, 1]])

# For right wrist initial position
const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.23],  #  right
                                       [0, 1, 0, 1.23],   #  height
                                       [0, 0, 1, -0.4],
                                       [0, 0, 0, 1]])


# 坐标系转换：从机器人坐标系（前左上）到机械臂末端坐标系（下左前）
# 需要将VR手柄的方向与机械臂末端的方向对齐，无需额外旋转90度
# 在机器人坐标系中：x轴指向前，y轴指向左，z轴指向上
# 在机械臂末端坐标系中：x轴指向下，y轴指向左，z轴指向前

# 左手腕的变换矩阵，将机器人坐标系转换为机械臂末端坐标系
T_to_fangnuo_left_wrist = np.array([[ 0.0, 0.0, 1.0, 0.0],  # x轴指向上变为指向前
                                   [ 0.0, 1.0, 0.0, 0.0],  # y轴保持不变，仍指向左
                                   [-1.0, 0.0, 0.0, 0.0],  # z轴指向前变为指向下
                                   [ 0.0, 0.0, 0.0, 1.0]])

# 右手腕使用相同的变换
T_to_fangnuo_right_wrist = np.array([[ 0.0, 0.0, 1.0, 0.0],  # x轴指向上变为指向前
                                    [ 0.0, 1.0, 0.0, 0.0],  # y轴保持不变，仍指向左
                                    [-1.0, 0.0, 0.0, 0.0],  # z轴指向前变为指向下
                                    [ 0.0, 0.0, 0.0, 1.0]])



T_to_fangnuo_hand = np.array([[0, 0, 1, 0],
                              [-1,0, 0, 0],
                              [0, -1,0, 0],
                              [0, 0, 0, 1]])

T_robot_openxr = np.array([[0, 0, -1, 0],
                           [-1, 0, 0, 0],
                           [0, 1, 0, 0],
                           [0, 0, 0, 1]])





# legacy
# const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.5],
#                                        [0, 1, 0, 1],
#                                        [0, 0, 1, -0.5],
#                                        [0, 0, 0, 1]])

# const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.5],
#                                       [0, 1, 0, 1],
#                                       [0, 0, 1, -0.5],
#                                       [0, 0, 0, 1]])
